#include "StdAfx.h"
#include "Curve.h"
#include "ContourDot.h"
#include "Stroke.h"
#include <GdiPlus.h>
#include <cmath>

// Constants
const float kLowerParameterLimit = 0.6f;
const float kUpperParameterLimit = 5.0f;


Curve::Curve(const ContourDot& inDot1, const ContourDot& inDot2,
	const Stroke* inStroke, int inStrokeEnd)
	:	mAx(inDot1.GetX()), mAy(inDot1.GetY()),
		mBx(inDot2.GetX()), mBy(inDot2.GetY()),
		mOwnerStroke(inStroke), mStrokeEnd(inStrokeEnd)
{
	float dx = mBx - mAx;
	float dy = mBy - mAy;
	float distanceSquared = dx * dx + dy * dy;
	float distance = ::sqrt(distanceSquared);

	if (Stroke::kAfterEnd == inStrokeEnd)
	{
		// the dots are going in reverse order, this can be accounted
		// by taking all the tangent vectors (GetVx(), GetVy()) as opposite.
		// Equivalently, for the purpose of calculation of the middle points,
		// we can negate the "distance" variable.
		distance = -distance;
	}

	float l1 = 2 * distanceSquared / 
		(3 * (distance + dx * inDot1.GetVx() + dy * inDot1.GetVy()));
	float l2 = 2 * distanceSquared / 
		(3 * (distance + dx * inDot2.GetVx() + dy * inDot2.GetVy()));

	mM1x = mAx + inDot1.GetVx() * l1;
	mM1y = mAy + inDot1.GetVy() * l1;
	mM2x = mBx - inDot2.GetVx() * l2;
	mM2y = mBy - inDot2.GetVy() * l2;
}

Curve::Curve(const Stroke* inStroke, int inStrokeEnd)
	: mOwnerStroke(inStroke), mStrokeEnd(inStrokeEnd)
{
	if (const ContourDot* dot = inStroke->GetContourDotAt(inStrokeEnd))
	{
		float eta = inStroke->GetTerminatingEta(inStrokeEnd);
		float k = dot->GetCurvature();
		float vx = dot->GetVx();
		float vy = dot->GetVy();
		float deltaX[3], deltaY[3];

		if (::fabs(k) < 0.01f)
		{
			float dx = eta * vx;
			float dy = eta * vy;
			deltaX[0] = 0.3f * dx;
			deltaY[0] = 0.3f * dy;
			deltaX[1] = 0.7f * dx;
			deltaY[1] = 0.7f * dy;
			deltaX[2] = dx;
			deltaY[2] = dy;
		}
		else
		{
			float angle = k * eta;
			float c = ::cos(angle), s = ::sin(angle);
			float ux = vx * c - vy * s;
			float uy = vx * s + vy * c;
			float l = 4.0f * tan(angle * 0.25f) / (3.0f * k);
			deltaX[0] = vx * l;
			deltaY[0] = vy * l;
			deltaX[2] = (uy - vy) / k;
			deltaY[2] = (vx - ux) / k;
			deltaX[1] = deltaX[2] - ux * l;
			deltaY[1] = deltaY[2] - uy * l;
		}

		if (Stroke::kBeforeFront == inStrokeEnd)
		{
			mBx = dot->GetX();
			mBy = dot->GetY();
			mM2x = mBx + deltaX[0];
			mM2y = mBy + deltaY[0];
			mM1x = mBx + deltaX[1];
			mM1y = mBy + deltaY[1];
			mAx = mBx + deltaX[2];
			mAy = mBy + deltaY[2];
		}
		else
		{
			mAx = dot->GetX();
			mAy = dot->GetY();
			mM1x = mAx + deltaX[0];
			mM1y = mAy + deltaY[0];
			mM2x = mAx + deltaX[1];
			mM2y = mAy + deltaY[1];
			mBx = mAx + deltaX[2];
			mBy = mAy + deltaY[2];
		}
	}
	else // if NULL == dot
	{
		// ? empty stroke
		mAx = mAy = mBx = mBy = mM1x = mM1y = mM2x = mM2y = 0.0f;
	}
}

Curve::Curve(float inX, float inY, float inVx, float inVy, 
	const Stroke* inStroke, int inStrokeEnd)
	: mAx(inX), mAy(inY), 
	mOwnerStroke(inStroke), mStrokeEnd(inStrokeEnd)
{
	const ContourDot* dot = inStroke->GetContourDotAt(inStrokeEnd);
	mBx = dot->GetX();
	mBy = dot->GetY();

	// make vector, directed towards the curve interior.
	float dotVx, dotVy;
	if (Stroke::kBeforeFront == inStrokeEnd)
	{
		dotVx = - dot->GetVx();
		dotVy = - dot->GetVy();
	}
	else
	{
		dotVx = dot->GetVx();
		dotVy = dot->GetVy();
	}

	float dx = mBx - mAx;
	float dy = mBy - mAy;
	float distanceSquared = dx * dx + dy * dy;
	float distance = ::sqrt(distanceSquared);
	float l1 = 2 * distanceSquared / 
		(3 * (distance + dx * inVx + dy * inVy));
	float l2 = 2 * distanceSquared / 
		(3 * (distance - dx * dotVx - dy * dotVy));

	mM1x = mAx + inVx * l1;
	mM1y = mAy + inVy * l1;
	mM2x = mBx + dotVx * l2;
	mM2y = mBy + dotVy * l2;
}

void Curve::AddToPath(Gdiplus::GraphicsPath* outPath) const
{
	outPath->AddBezier(mAx, mAy, mM1x, mM1y, mM2x, mM2y, mBx, mBy);
}


void Curve::GetPointAtParameter(float inT, float& outX, float& outY) const
{
	float tau = 1.0f - inT;
	float t2 = inT * inT;
	float tau2 = tau * tau;
	float k1 = tau2 * tau;
	float k2 = 3 * inT * tau2;
	float k3 = 3 * t2 * tau;
	float k4 = t2 * inT;
	outX = mAx * k1 + mM1x * k2 + mM2x * k3 + mBx * k4;
	outY = mAy * k1 + mM1y * k2 + mM2y * k3 + mBy * k4;
}

void Curve::GetDirectionAtParameter(float inT, float& outVx, float& outVy) const
{
	float tau = 1.0f - inT;
	float t2 = inT * inT;
	float tau2 = tau * tau;
	float twottau = 2 * inT * tau;
	float k1 = -3 * tau2;
	float k2 = 3 * (tau2 - twottau);
	float k3 = 3 * (twottau - t2);
	float k4 = 3 * t2;
	outVx = mAx * k1 + mM1x * k2 + mM2x * k3 + mBx * k4;
	outVy = mAy * k1 + mM1y * k2 + mM2y * k3 + mBy * k4;
}

const Stroke* Curve::GetStroke() const
{
	return mOwnerStroke;
}

int Curve::GetStrokeEnd() const
{
	return mStrokeEnd;
}


bool Curve::AreDotsSeparated(const ContourDot& inDot1,
	const ContourDot& inDot2) const
{
	// use approximation of curve with a straight line
	// TODO: make more accurate calculations
	float dx = mBx - mAx;
	float dy = mBy - mAy;
	float dx1 = inDot1.GetX() - mAx;
	float dy1 = inDot1.GetY() - mAy;
	float dx2 = inDot2.GetX() - mAx;
	float dy2 = inDot2.GetY() - mAy;
	float vectorProduct1 = dx1 * dy - dy1 * dx;
	float vectorProduct2 = dx2 * dy - dy2 * dx;
	return (0 > vectorProduct1 * vectorProduct2);
}

float Curve::FindParameterAimedByContourDot(const ContourDot& inDot,
	bool inReverseDotDirection) const
{
	float x0 = inDot.GetX();
	float y0 = inDot.GetY();
	float vx = inDot.GetVx();
	float vy = inDot.GetVy();
	float kh = 0.5f * inDot.GetCurvature();

	if (inReverseDotDirection)
	{
		vx = -vx;
		vy = -vy;
		kh = -kh;
	}

	float ax = mAx - x0;
	float ay = mAy - y0;
	float bx = mBx - x0;
	float by = mBy - y0;

	// should be an acute angle between the contour dot's tangent vector v
	// and the direction towards the curve's middle.
	if (0 >= (vx * (ax + bx) + vy * (ay + by)))
	{
		return -1.0f;
	}

	float f1 = vx * ay - vy * ax - kh * (ax * ax + ay * ay);
	float f2 = vx * by - vy * bx - kh * (bx * bx + by * by);

	if (f1 * f2 > 0)
	{
		return -1;
	}

	float t1 = 0, t2 = 1;
	const float epsilon = 0.01f;
	int iterationLimit = 10;
	while (t2 - t1 > epsilon && f2 != f1)
	{
		float t = (f2 * t1 - f1 * t2) / (f2 - f1);
		float x, y;
		GetPointAtParameter(t, x, y);
		float dx = x - x0;
		float dy = y - y0;
		float f = vx * dy - vy * dx - kh * (dx * dx + dy * dy);
		if (f * f1 < 0)
		{
			t2 = t;
			f2 = f;
		}
		else
		{
			t1 = t;
			f1 = f;
		}

		if (0 > --iterationLimit)
		{
			if (::fabs(f2) < ::fabs(f1))
			{
				t1 = t2;
			}
			break;
		}
	}

	return t1;
}
